
#include "vector.h"

#include <stdlib.h>
#include <math.h>

#define _COS_GRAN 5001

double _cos_lookup[_COS_GRAN];
int _cos_iterations;
int _cos_start_ind;
int _cos_init_dindex;

vm_vector UNIT_X;
vm_vector UNIT_Y;
vm_vector UNIT_Z;
vm_vector UNIT_W;

double log2(double x);

void _load_cos_lookup_table() {

  _cos_start_ind = (int) (_COS_GRAN / 2);
  _cos_iterations = (int) (log2(_cos_start_ind) + 1);
  _cos_init_dindex = (int)(_cos_start_ind / 2);

  //printf("_COS_GRAN=%d\n", _COS_GRAN);
  //printf("_cos_start_ind=%d\n", _cos_start_ind);
  //printf("_cos_iterations=%d\n", _cos_iterations);
  //printf("_cos_init_dindex=%d\n", _cos_init_dindex);

  int i = 0;
  double theta = 0;
  double dtheta = M_PI / (_COS_GRAN - 1);

  for ( ; i < _COS_GRAN; i++){
    _cos_lookup[i] = cos(theta);
    //fprintf(stderr, "theta=%f  cos(theta)=%f (index=%d)\n", 
	    //theta,_cos_lookup[i], i);
    theta += dtheta;
  }

}

double _vm_cos(double theta){

  if (theta < 0) theta *= -1;
  if (theta > M_PI) return -2;

  //printf("[_vm_cos] index value: %d\n", (int) (theta * _COS_GRAN / M_PI));

  return _cos_lookup[(int) (theta * _COS_GRAN / M_PI)];

}

int _vm_inv_cos(double ctheta, double *theta){

  char neg = 0; // 1 if the ctheta value is positive. 0 otherwise.

  if (ctheta < 0) { // Make ctheta positive; makes things easier.
    ctheta *= -1;
    neg = 1;        // But make sure we remeber that ctheta was negative.
  }
  /* Test ctheta's bounds. 0 <= ctheta <= 1 at this point */
  if (ctheta < 0 || ctheta > 1) return 0;

  // Now find cos(theta) in the look uptable
  int index = _cos_start_ind; // Start in the middle.
  int dindex = _cos_init_dindex;
  int i = 0;       
  for ( ; i < _cos_iterations; i++){
    if (ctheta > _cos_lookup[index]){
      index -= dindex;
    } else {
      index += dindex;
    }
    dindex /= 2;
    //printf("loop=%d  index=%d\n", i, index);
  }
  //printf("overall index: %d\n", index);
  *theta = M_PI * index / (_COS_GRAN - 1);
  if (neg){
    *theta = M_PI - *theta; 
  }
  return 1;

}

void _init(){
  _load_cos_lookup_table();
  X_AXIS(UNIT_X);
  Y_AXIS(UNIT_Y);
  Z_AXIS(UNIT_Z);
  W_AXIS(UNIT_W);
}

void vm_add(vm_vector v1, vm_vector v2, vm_vector *dest){

  dest->x = v1.x + v2.x;
  dest->y = v1.y + v2.y;
  dest->z = v1.z + v2.z;
  dest->w = v1.w + v2.w;

}

void vm_sub(vm_vector v1, vm_vector v2, vm_vector *dest){

  dest->x = v1.x - v2.x;
  dest->y = v1.y - v2.y;
  dest->z = v1.z - v2.z;
  dest->w = v1.w - v2.w;

}

void vm_scale(vm_vector v, double factor, vm_vector *dest){

  dest->x = v.x * factor;
  dest->y = v.y * factor;
  dest->z = v.z * factor;
  dest->w = v.w * factor;

}

double vm_magnitude(vm_vector v){

  return sqrt((v.x * v.x) + (v.y * v.y) + (v.z * v.z) + (v.w * v.w));

}

void vm_norm(vm_vector v, vm_vector *dest){

  double v_mag = vm_magnitude(v);
  vm_scale(v, 1.0 / v_mag, dest);

}

void vm_cross(vm_vector v1, vm_vector v2, vm_vector *dest){

  dest->x = (v1.y * v2.z) - (v1.z * v2.y);
  dest->y = (v1.z * v2.x) - (v1.x * v2.z);
  dest->z = (v1.x * v2.y) - (v1.y * v2.x);

}

/**
 * Project v1 onto v2.
 */
void vm_project(vm_vector v1, vm_vector v2, vm_vector *dest){

  double dot;
  vm_dot(v1, v2, &dot);
  double mag = vm_magnitude(v2);
  vm_scale(v2, (dot / (mag * mag)), dest);

}

void vm_dot(vm_vector v1, vm_vector v2, double *dest){

  *dest = (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z) + (v1.w * v2.w);

}

double vm_angle(vm_vector v1, vm_vector v2){

  double dot;
  vm_dot(v1, v2, &dot);
  double div = vm_magnitude(v1) * vm_magnitude(v2);
  double cos_theta = dot / div;
  double theta;
  _vm_inv_cos(cos_theta, &theta);
  return theta;
  
}

void vm_vect_inverse(vm_vector v, vm_vector *dest){

  double mag = vm_magnitude(v);
  //fprintf(stderr, "Mag: %f\n", mag);
  //fflush(stderr);
  vm_scale(v, 1 / (mag * mag), dest);
  return;

}

vm_vector *vm_gram_schmidt_orth(vm_vector *vects, int len){

  vm_vector *b = (vm_vector *)malloc(sizeof(vm_vector) * len);
  vm_vector tmp;
  int i = 1;

  b[0] = vects[0];

  for ( ; i < len; i++){
    /* First calulate the sum of the previous vectors projections. */
    ZERO(tmp);
    int j = 0;
    vm_vector proj;
    for ( ; j < i; j++){
      vm_project(vects[i], b[j], &proj);
      vm_add(tmp, proj, &tmp);
    }
    /* Now subtract it from the base vector. */
    vm_sub(vects[i], tmp, &(b[i]));
  }

  for ( i = 0; i < len; i++){
    vm_norm(b[i], &(b[i]));
  }

  return b;

}

void vm_print(vm_vector v){

  printf("[%f:%f:%f:%f]", v.x, v.y, v.z, v.w);

}

void vm_println(vm_vector v){

  printf("[%f:%f:%f:%f]\n", v.x, v.y, v.z, v.w);

}

void vm_fprint(FILE *f, vm_vector v){

  fprintf(f, "[%f:%f:%f:%f]", v.x, v.y, v.z, v.w);

}

void vm_fprintln(FILE *f, vm_vector v){

  fprintf(f, "[%f:%f:%f:%f]\n", v.x, v.y, v.z, v.w);

}

int vm_read(FILE *file, vm_vector *v){

  float x, y, z, w;

  int conv = fscanf(file, "[%f:%f:%f:%f]\n", &x, &y, &z, &w);
  //printf("[vm_read] x=%f y=%f z=%f\n", x, y, z);
  v->x = (double) x;
  v->y = (double) y;
  v->z = (double) z;
  v->w = (double) w;
  //printf("[vm_read] v->x=%f v->y=%f v->z=%f\n", v->x, v->y, v->z);
  if (conv != 4){
    return 0;
  }
  return 1;

}

int vm_sread(char *str, vm_vector *v){

  float x, y, z, w;

  int conv = sscanf(str, "[%f:%f:%f:%f]", &x, &y, &z, &w);
  //printf("[vm_sread] x=%f y=%f z=%f\n", x, y, z);
  v->x = (double) x;
  v->y = (double) y;
  v->z = (double) z;
  v->w = (double) w;
  //printf("[vm_sread] v->x=%f v->y=%f v->z=%f\n", v->x, v->y, v->z);
  if (conv != 4){
    return 0;
  }
  return 1;

}
